#ifndef __UBT_UTILS_H__
#define __UBT_UTILS_H__

#include <iostream>
#include <fstream>
#include <filesystem>
#include <sstream>
#include <utility>
#include <string>
#include <ctime>
#include <chrono>
#include <vector>
#include <jsoncpp/json/json.h>
#include <opencv2/opencv.hpp>
#include <zip.h>
#include <ubt_mqtt/node.h> 

namespace ubt {

using namespace std::chrono;

class ubt_utils
{
public:
    /**
     * @brief Get the Time Str object
     * 
     * @return std::string (such as: 2021-11-03 11:40:14)
     */
    static std::string getTimeStr()
    {
        char timeStr[128] = {0};
        auto now = system_clock::now();
        //auto now_seconds = now.time_since_epoch();

        //uint64_t dis_ms = duration_cast<milliseconds>(now_seconds).count() - (duration_cast<seconds>(now_seconds).count() * 1000);

        time_t tt = system_clock::to_time_t(now);
        auto time_tm = localtime(&tt);

        if (std::strftime(timeStr, sizeof(timeStr), "%Y-%m-%d %H:%M:%S", time_tm))
        {
            //return std::string(timeStr) + "." + std::to_string(dis_ms);
            return std::string(timeStr);
        }

        return "";
    }

    /**
     * @brief Get the Time Milliseconds object
     * 
     * @return uint64_t 
     */
    static uint64_t getTimeMilliseconds()
    {
        return duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
    }

    /**
     * @brief 字符串分割函数
     * @param str
     * @param pattern
     * @return std::vector<std::string>
     */
    static std::vector<std::string> split(const std::string &str, std::string pattern)
    {
        std::vector<std::string> resVec;
        std::string strs = str + pattern;
    
        size_t pos = strs.find(pattern);
        size_t size = strs.size();

        while (pos != std::string::npos)
        {
            std::string x = strs.substr(0, pos);
            resVec.push_back(x);
            strs = strs.substr(pos + 1, size);
            pos = strs.find(pattern);
        }

        return resVec;
    }
    /**
     * @brief 获取文件数据
     * @param pathYaml
     * @param key
     */
    static Json::Value getFileData(const std::string& pathYaml) 
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"getFileData, path: " << pathYaml);
        std::ifstream file;
        Json::Value result;
        file.exceptions(std::ifstream::failbit | std::ifstream::badbit);

        try {
            file.open(pathYaml);
            std::string line;

            while (std::getline(file, line)) {
                std::istringstream iss(line);
                std::string key, value;
                
                if (std::getline(iss, key, ':') && std::getline(iss, value)) {
                    // 避免文件为空的时候数据越界
                    result[key] = value;
                }
            }
            if (file.is_open()) {
                file.close();
            }
        } catch (const std::exception& e) {
            RCLCPP_INFO_STREAM(node->get_logger(),"getFileData err: " << e.what());
        }

        RCLCPP_INFO_STREAM(node->get_logger(),"getFileData: " << result);

        return result;
    }

    /**
     * @brief 获取地图原始信息
     * @param map_origin_yaml
     * @param origin_key
     * @return std::vector<std::string>
     */
    static std::vector<std::string> getMapOriginInfo(const std::string& map_origin_yaml, const std::string& origin_key) 
    {
        std::vector<std::string> result;
        try {
            Json::Value res = getFileData(map_origin_yaml);

            if (res.isObject() && res.isMember(origin_key)) {
                for (unsigned int i = 0; i < res[origin_key].size(); ++i) {
                    result.push_back(res[origin_key][i].asString());
                }
            }
        } catch (const std::exception& e) {
            RCLCPP_INFO_STREAM(node->get_logger(),"get_origin_map_info err = " << e.what());
        }

        return result;
    }
    
    std::vector<std::string> getTaskYamlInfo(const std::string& task_yaml) {
        std::vector<std::string> task_data{"", "", ""};

        try {
            // 解析 task_yaml
            Json::Value res = getFileData(task_yaml);
            if (!res.isNull() && res.isObject() && res.isMember("task")) {
                Json::Value task_json = res["task"];
                // 假设 task_json 中的数据为字符串数组
                for (int i = 0; i < std::min(static_cast<int>(task_data.size()), static_cast<int>(task_json.size())); ++i) {
                    if (task_json[i].isString()) {
                        task_data[i] = task_json[i].asString();
                    }
                }
            }
        } catch (const std::exception& e) {
            RCLCPP_WARN_STREAM(node->get_logger(),"get_task_yaml_info error: " << e.what());
        }

        return task_data;
    }

        /**
     * @brief json 字串解析
     * @param jsonData
     * @param root
     * @return true
     * @return false
     */
    static bool PraseJsonString(const std::string& jsonData, Json::Value& root)
    {
        JSONCPP_STRING err;
        Json::CharReaderBuilder builder;
        const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
        const auto rawJsonLength = static_cast<int>(jsonData.length());

        if (!reader->parse(jsonData.c_str(), jsonData.c_str() + rawJsonLength, &root, &err))
        {
            RCLCPP_INFO_STREAM(node->get_logger(),"json prase error, data: " << jsonData << " err info: " << err);
            return false;
        }

        return true;
    }

    /**
     * @brief 更新地图配置 yaml 文件
     * @param map_data: 地图数据
     * @param map_image_path: 地图存储路径
     * @param map_origin_path: 地图配置文件路径
     * @return bool
     */
    static bool updateOriginYamlFile(const Json::Value& map_data, const std::string& map_image_path, const std::string& map_origin_path) 
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"map image path: " << map_image_path << ", map orgin path: " << map_origin_path);

        if (map_data["map_type"] != "QRCODE") {
            try {
                std::string yamlString = map_data["yaml"].asString();
                Json::CharReaderBuilder builder;
                Json::CharReader* reader = builder.newCharReader();
                        Json::Value yaml;
                std::istringstream yamlStream(yamlString);
                Json::parseFromStream(builder, yamlStream, &yaml, nullptr);
                
                        double occupied_thresh = yaml["occupied_thresh"].asDouble();
                        bool negate = yaml["negate"].asBool();
                        Json::Value origin = yaml["origin"];
                        double free_thresh = yaml["free_thresh"].asDouble();
                        double resolution = yaml["resolution"].asDouble();

                Json::FastWriter writer;
                        std::string orgin_yaml_data = "image: " + map_image_path + '\n'
                            + "resolution: " + std::to_string(resolution) + "free_thresh: " + std::to_string(free_thresh)
                            + "occupied_thresh: " + std::to_string(occupied_thresh) + "origin: " + writer.write(origin) + '\n'
                            + "negate: " + std::to_string(negate) + '\n';

                        std::ofstream file(map_origin_path, std::ios::binary);
                        file << orgin_yaml_data;
                delete reader;
            } catch (const std::exception& e) {
                RCLCPP_INFO_STREAM(node->get_logger(),"read mission yaml errcode = " << e.what());
                return false;
            }
        } else {
            std::ofstream file(map_origin_path, std::ios::binary);
            file << '\n';
        }

        std::vector<std::string> map_list = {
            map_data["map_id"].asString(),
            map_data["map_name"].asString(),
            map_data["map_code"].asString()
        };

        std::ofstream file(map_origin_path, std::ios::binary | std::ios::app);
	for (const auto& str : map_list){
		file << str << std::endl;
	}
        // file << "map: " << Json::writeString(map_list) << '\n';
	Json::StreamWriterBuilder writerBuilder;
        file << "map_hash: " << Json::writeString(writerBuilder, map_data["map_hash"]) << '\n';

        return true;
    }

    /**
     * @brief 储存地图图像
     * @param mapPng : 地图
     * @param mapImagePath : 地图路径
     * @return bool
     */
    static bool directlySaveMapImage(const std::string& mapPng, const std::string& mapImagePath) {
        try {
            std::ofstream file(mapImagePath, std::ios::binary);
            file.write(mapPng.c_str(), mapPng.size());
            std::cout << "Image written to " << mapImagePath << std::endl;
        } catch (const std::exception& e) {
            std::cerr << "directlySaveMapImage error: " << e.what() << std::endl;
            return false;
        }

        try {
            // Read the image data from the file using imread
            cv::Mat imgOrgin = cv::imread(mapImagePath, cv::IMREAD_UNCHANGED);
            // Save the formatted image data back to the file
            cv::imwrite(mapImagePath, imgOrgin, {cv::IMWRITE_JPEG_QUALITY, 0});
        } catch (const cv::Exception& e) {
            std::cerr << "cv::imwrite error: " << e.what() << std::endl;
            return false;
        }

        return true;
    }

    static bool saveMapImage3D(const std::string& map3d_zip_file, const std::string& map3d_path) {
        std::string map3d_path_zip = map3d_path + "had_map.zip";
        try {
            // 写入 zip 文件
            std::ofstream file(map3d_path_zip, std::ios::binary);
            if (!file.is_open()) {
                std::cerr << "Failed to open file: " << map3d_path_zip << std::endl;
                return false;
            }
            file.write(map3d_zip_file.c_str(), map3d_zip_file.size());
            file.close();

            // 解压缩 zip 文件
            std::string unzip_cmd = "unzip " + map3d_path_zip + " -d " + map3d_path;
            if (std::system(unzip_cmd.c_str()) != 0) {
                std::cerr << "Failed to unzip file: " << map3d_path_zip << std::endl;
                return false;
            }
            std::cout << "Zip file of 3dmap successfully decompressed" << std::endl;

            // 删除 zip 文件
            std::string rm_cmd = "rm " + map3d_path_zip;
            if (std::system(rm_cmd.c_str()) != 0) {
                std::cerr << "Failed to delete file: " << map3d_path_zip << std::endl;
                return false;
            }
            std::cout << "Zip file of 3dmap deleted successfully" << std::endl;

        } catch (const std::exception& e) {
            std::cerr << "save_map_image_3d error: " << e.what() << std::endl;
            return false;
        }

        return true;
    }

    static std::vector<unsigned char> base64_decode(const std::string &encoded_string) {
        std::vector<unsigned char> decoded_data;
        
        const std::string base64_chars =
                "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
                "abcdefghijklmnopqrstuvwxyz"
                "0123456789+/";
        
        int in_len = encoded_string.size();
        int i = 0, j = 0, in_ = 0;
        unsigned char char_array_4[4], char_array_3[3];
        
        while (in_len-- && ( encoded_string[in_] != '=') && (isalnum(encoded_string[in_]) || (encoded_string[in_] == '+') || (encoded_string[in_] == '/'))) {
            char_array_4[i++] = encoded_string[in_]; in_++;
            if (i ==4) {
                for (i = 0; i <4; i++)
                    char_array_4[i] = base64_chars.find(char_array_4[i]);
                
                char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
                char_array_3[1] = ((char_array_4[1] & 0x0F) << 4) + ((char_array_4[2] & 0x3C) >> 2);
                char_array_3[2] = ((char_array_4[2] & 0x03) << 6) + char_array_4[3];
                
                for (i = 0; (i < 3); i++)
                    decoded_data.push_back(char_array_3[i]);
                i = 0;
            }
        }
        
        if (i) {
            for (j = i; j <4; j++)
                char_array_4[j] = 0;
            
            for (j = 0; j <4; j++)
                char_array_4[j] = base64_chars.find(char_array_4[j]);
            
            char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
            char_array_3[1] = ((char_array_4[1] & 0x0F) << 4) + ((char_array_4[2] & 0x3C) >> 2);
            char_array_3[2] = ((char_array_4[2] & 0x03) << 6) + char_array_4[3];
            
            for (j = 0; (j < i - 1); j++) decoded_data.push_back(char_array_3[j]);
        } 
        
        return decoded_data;
    }

    static int calcBatteryLevel(int realLevel)
    {
        int batteryLevel = 0;

        if (realLevel < 0 || realLevel > 100)
        {
            std::cerr << "Battery exception, level: " << realLevel << std::endl;
            return -1;
        }

        if (realLevel == 99)
        {
            batteryLevel = 100;
        }
        else
        {
            batteryLevel = static_cast<int>((realLevel - 6.0) / (100.0 - 6.0) * 100.0 + 0.5);
        }

        if (batteryLevel < 0)
        {
            batteryLevel = 0;
        }
        else if (batteryLevel > 100)
        {
            batteryLevel = 100;
        }

        // std::cout << "Battery exception, real level: " << realLevel << ", cur level: " << batteryLevel << std::endl;
        return batteryLevel; // 返回整数百分比电量
    }



    static bool saveMapImage(const std::string& mapPng, const std::string& mapImagePath) {
        // Remove the base64 header
        std::string baseMap = mapPng.substr(mapPng.find(',') + 1);

        // Decode base64 string
        try {
            // std::istringstream iss(baseMap);
            std::vector<unsigned char> decodedMapVec = base64_decode(baseMap);
	    std::string decodedMap(decodedMapVec.begin(), decodedMapVec.end());
            // Save the decoded image data to a file
            std::ofstream file(mapImagePath, std::ios::binary);
            file.write(decodedMap.c_str(), decodedMap.length());
            std::cout << "Image saved successfully to " << mapImagePath << std::endl;

            // Read and re-save the image using OpenCV to ensure format consistency
            cv::Mat imgOrigin = cv::imdecode(decodedMapVec, cv::IMREAD_UNCHANGED);
            cv::imwrite(mapImagePath, imgOrigin, {cv::IMWRITE_JPEG_QUALITY, 0});

            std::cout << "Image re-saved successfully to " << mapImagePath << std::endl;
        } catch (const std::exception& e) {
            std::cerr << "saveMapImage error: " << e.what() << std::endl;
            return false;
        }

        return true;
    }
    
    /**
     * @brief 解析地图文件,获取地图长/宽
     * @param pathMap : 地图路径
     * @return std::pair<int, int>
     */
    static std::pair<float, float> getMapInfo(const std::string& pathMap) {
        try {
            if (std::filesystem::exists(pathMap) && std::filesystem::file_size(pathMap) > 0) {
                cv::Mat img = cv::imread(pathMap, cv::IMREAD_GRAYSCALE);
                if (!img.empty()) {
                    return {static_cast<float>(img.cols), static_cast<float>(img.rows)};
                }
            }
        } catch (const std::exception& e) {
            RCLCPP_WARN_STREAM(node->get_logger(),"getMapInfo error: " << e.what());
        }

        return {0, 0};
    }
    /**
     * @brief 字符串替换(全部替换)
     * @param str : 原始数据
     * @param oldstr : 要被替换的字串
     * @param newstr : 新的字串
     * @return std::string&
     */
    static std::string& ReplaceStringAll(std::string& str, const std::string& oldstr, const std::string& newstr)
    {
        do
        {
            std::string::size_type pos(0);
            if ((pos = str.find(oldstr)) != std::string::npos)
            {
                str.replace(pos, oldstr.length(), newstr);
            }
            else
            {
                break;
            }
        } while (true);

        return str;
    }
};

}

#endif

